// --------------------------------------------------------------------------------------------------------------------
// <copyright file="FriSimulator.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Hardware.KukaLBR.KukaLBR4Tests
{
    using System;
    using System.Diagnostics;
    using System.IO;
    using System.Reflection;
    using System.Threading;
    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Simulation KUKA FRI for testing
    /// </summary>
    public class FriSimulator : IManagedFRI
    {
        /// <summary>
        /// Default update rate of FRI
        /// </summary>
        private const int DefaultFRIUpdateCycleInMs = 100;

        /// <summary>
        /// Simulation generator
        /// </summary>
        private Random rand;

        /// <summary>
        /// Generates timestamps
        /// </summary>
        private Stopwatch timer;

        /// <summary>
        /// Initializes a new instance of the <see cref="FriSimulator"/> class
        /// </summary>
        public FriSimulator()
        {
            this.Initialize();
        }

        /// <summary>
        /// Gets Count of calls to Do Nothing API
        /// </summary>
        public int DoNothingCount { get; private set; }

        /// <summary>
        /// Gets Count of calls to GetRequestCount API
        /// </summary>
        public int GetRequestCount { get; private set; }

        /// <summary>
        /// Gets Count of calls to WaitOnConnectCount API
        /// </summary>
        public int WaitOnConnectCount { get; private set; }

        /// <summary>
        /// Gets Count of calls to Initialize API
        /// </summary>
        public int InitializeCount { get; private set; }

        /// <summary>
        /// Gets Count of calls to Close API
        /// </summary>
        public int CloseCount { get; private set; }

        /// <summary>
        /// Gets Count of calls to DoPositionControl API
        /// </summary>
        public int DoPositionControlCount { get; private set; }

        /// <summary>
        /// Gets Count of calls to DoJointImpedanceControl API
        /// </summary>
        public int DoJointImpedanceControlCount { get; private set; }

        /// <summary>
        /// Gets Count of calls to DoCartesianImpedanceControl API
        /// </summary>
        public int DoCartesianImpedanceControlCount { get; private set; }

        /// <summary>
        /// Gets Count of calls to DoKrlCartesianImpedanceControl API
        /// </summary>
        public int DoKrlCartesianImpedanceControlCount { get; private set; }

        /// <summary>
        /// Gets Joint positions set
        /// </summary>
        public double[] JointPositions { get; private set; }

        /// <summary>
        /// Gets the last set cartesian position
        /// </summary>
        public double[] CurrentCartesianPosition { get; private set; }

        /// <summary>
        /// Gets the last commanded cartesian control position
        /// </summary>
        public double[] CommandedCartesianPosition { get; private set; }

        /// <summary>
        /// Gets the last set joint position
        /// </summary>
        public double[] JountControlPositions { get; private set; }

        /// <summary>
        /// Gets the last commanded joint control position
        /// </summary>
        public double[] CommandedJointPosition { get; private set; }

        /// <summary>
        /// Gets the commanded cartesian offset based on FRI command (will always
        /// be identity rotation and zero translation in sim).
        /// </summary>
        public double[] CommandedCartesianPositionOffset { get; private set; }

        /// <summary>
        /// Gets the state of the FRI connection
        /// </summary>
        public double FRIstate { get; private set; }

        /// <summary>
        /// Gets Cycle time of FRI in mSec
        /// </summary>
        public int CycleTimeinMsec { get; private set; }

        /// <summary>
        /// Gets or sets the TCP force to apply.
        /// </summary>
        public Vector3 TcpForce { get; set; }

        /// <summary>
        /// Implementation of the interface member
        /// </summary>
        /// <returns>0 on success</returns>
        public int InitializeFRI()
        {
            ++this.InitializeCount;
            return 0;
        }

        /// <summary>
        /// Implementation of the interface member
        /// </summary>
        /// <returns>0 on success</returns>
        public int CloseFRI()
        {
            ++this.CloseCount;
            return 0;
        }

        /// <summary>
        /// Implementation of the interface member
        /// </summary>
        /// <returns>0 on success</returns>
        public int DoNothing()
        {
            ++this.DoNothingCount;
            return 0;
        }

        /// <summary>
        /// Implementation of the interface member Returns state of FRI in the registers
        /// </summary>
        /// <returns>0 on success</returns>
        /// <param name="feedbackArrayPtr">Array of feedback register  buffers</param>
        public unsafe virtual int GetFRIRequest(double** feedbackArrayPtr)
        {
            ++this.GetRequestCount;

            int cartesianPositionMeasuredRegister = (int)KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusFeedbackRegisters.MeasuredCartesianPositionRegister;
            int cartesianPositionCommandedRegister = (int)KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusFeedbackRegisters.CommandedCartesianPositionRegister;
            int cartesianPositionCommandedOffestRegister = (int)KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusFeedbackRegisters.CommandedCartesianOffsetRegister;
            int jointPositionMeasuredRegister = (int)KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusFeedbackRegisters.MeasuredJointPositionRegister;
            int jointPositionCommandedRegister = (int)KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusFeedbackRegisters.CommandedJointPositionRegister;
            int tcpForceTorqueRegister = (int)KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusFeedbackRegisters.EstimatedTCPForceTorqueRegister;
            int stateRegister = (int)KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusFeedbackRegisters.FRIStateRegister;
            int cycleTimeInMsRegister = (int)KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusFeedbackRegisters.CycleTimeInMsRegister;
            int timestampRegister = (int)KukaLBR4PlusCsharpDefinitionsNativeMethods.KukaLBR4PlusFeedbackRegisters.TimestampInMsRegister;

            feedbackArrayPtr[timestampRegister][0] = (double)this.timer.ElapsedMilliseconds;

            for (int i = 0; i < this.CurrentCartesianPosition.Length; ++i)
            {
                feedbackArrayPtr[cartesianPositionMeasuredRegister][i] = this.CurrentCartesianPosition[i];
                feedbackArrayPtr[cartesianPositionCommandedRegister][i] = this.CommandedCartesianPosition[i];
                feedbackArrayPtr[cartesianPositionCommandedOffestRegister][i] = this.CommandedCartesianPositionOffset[i];
            }

            for (int i = 0; i < this.JointPositions.Length; ++i)
            {
                feedbackArrayPtr[jointPositionMeasuredRegister][i] = this.JointPositions[i];
                feedbackArrayPtr[jointPositionCommandedRegister][i] = this.CommandedJointPosition[i];
            }

            feedbackArrayPtr[stateRegister][0] = this.FRIstate;

            feedbackArrayPtr[cycleTimeInMsRegister][0] = this.CycleTimeinMsec;

            feedbackArrayPtr[tcpForceTorqueRegister][0] = this.TcpForce.X;
            feedbackArrayPtr[tcpForceTorqueRegister][1] = this.TcpForce.Y;
            feedbackArrayPtr[tcpForceTorqueRegister][2] = this.TcpForce.Z;
            return 0;
        }

        /// <summary>
        /// Implementation of the interface member
        /// </summary>
        /// <returns>0 on success</returns>
        public int WaitOnFRIConnect()
        {
            ++this.WaitOnConnectCount;
            return 0;
        }

        /// <summary> Simulate joint position control. Moves immediately to indicated position.
        /// </summary>
        /// <param name="jointPosition"> Array of joint positions. </param>
        /// <returns> Status code.  0 on no error. </returns>
        public unsafe int DoPositionControl(double* jointPosition)
        {
            const double PositionVariability = 0.0001;
            ++this.DoPositionControlCount;

            for (int i = 0; i < this.JointPositions.Length; ++i)
            {
                this.JointPositions[i] = this.rand.NextDouble() * PositionVariability + jointPosition[i];
                this.CommandedJointPosition[i] = jointPosition[i];
            }

            return 0;
        }

        /// <summary> Execute joint impedance control. </summary>
        /// <param name="jointPosition"> Array of joint positions. </param>
        /// <param name="jointStiffness"> Array of joint stiffness values. </param>
        /// <param name="jointDamping"> Array of joint damping values. </param>
        /// <param name="jointAdditionalTorque"> Array of addition joint torque values. </param>
        /// <returns> Status code.  0 on no error. </returns>
        public unsafe int DoJointImpedanceControl(
            double* jointPosition,
            double* jointStiffness,
            double* jointDamping,
            double* jointAdditionalTorque)
        {
            ++this.DoJointImpedanceControlCount;
            return 0;
        }

        /// <summary> Execute Cartesian impedance control. Takes a random step, along the requested vector </summary>
        /// <param name="cartesianPosition"> Array indicating desired cartesian position relative to base frame. </param>
        /// <param name="cartesianStiffness"> Array of Cartesian stiffness values. </param>
        /// <param name="cartesianDamping"> Array of Cartesian damping values. </param>
        /// <param name="additionalTCPForceTorque"> Array of additional TCP force torque values. </param>
        /// <returns>0 on success</returns>
        public unsafe int DoCartesianImpedanceControl(
            double* cartesianPosition,
            double* cartesianStiffness,
            double* cartesianDamping,
            double* additionalTCPForceTorque)
        {
            ++this.DoCartesianImpedanceControlCount;
            for (int i = 0; i < this.CurrentCartesianPosition.Length; ++i)
            {
                this.CurrentCartesianPosition[i] = cartesianPosition[i];
                this.CommandedCartesianPosition[i] = cartesianPosition[i];
            }

            return 0;
        }

        /// <summary> Execute Cartesian impedance control intended to be executed by KRL motion commands. </summary>
        /// <param name="cartesianPosition"> Array indicating desired cartesian position relative to base frame. </param>
        /// <param name="sequenceNumber"> Sequence number of the command. </param>
        /// <returns>0 on success</returns>
        public unsafe int DoKrlCartesianImpedanceControl(
            double* cartesianPosition,
            int sequenceNumber)
        {
            ++this.DoKrlCartesianImpedanceControlCount;

            // increment the same position values as normal impedance control as feedback
            // is same whether or not KRL is used to generate motions.
            for (int i = 0; i < this.CurrentCartesianPosition.Length; ++i)
            {
                this.CurrentCartesianPosition[i] = cartesianPosition[i];
                this.CommandedCartesianPosition[i] = cartesianPosition[i];
            }

            return 0;
        }

        /// <summary>
        /// Initialize the class
        /// </summary>
        private void Initialize()
        {
            this.rand = new Random(10);
            this.CommandedCartesianPositionOffset = new double[12] { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 };
            this.CurrentCartesianPosition = new double[12] { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 };
            this.CommandedCartesianPosition = new double[12] { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0 };
            this.JointPositions = new double[KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS];
            this.CommandedJointPosition = new double[KukaLBR4PlusCsharpDefinitionsNativeMethods.NUM_JOINTS];
            this.FRIstate = 2.0;
            this.CycleTimeinMsec = DefaultFRIUpdateCycleInMs;
            this.TcpForce = new Vector3(0, 0, 0);
            this.timer = new Stopwatch();
            this.timer.Start();
        }
    }
}
